#define JMBC_INSIDE
#include "mlFrustum.h"
#include <Core/clCore.h>
#include <math.h>
#include <GL/glu.h>

// Context

#define STORE ID_SMALL_STORE

static MLFrustum *store[STORE];

void
mlFrustumCreateContext(void)
{
	idClearStore(MLFrustum, STORE, store);
}

// Generation / Deletion

static
void
Init(MLFrustum *frustum)
{
	tlGenGroups(1, frustum);
	tlGroupAllocate(*frustum, 24, sizeof(float));
}

static
void
Term(MLFrustum *frustum)
{
	tlGroupUnload(*frustum);
	tlDeleteGroups(1, frustum);
}

void
mlGenFrustums(unsigned int n, unsigned int *frustums)
{
	idGenObjects(MLFrustum, STORE, store, Init, n, frustums);
}

void
mlDeleteFrustums(unsigned int n, unsigned int *frustums)
{
	idDeleteObjects(store, Term, n, frustums);
}

// Operation

void
mlFrustumCalc(unsigned int id)
{
	MLFrustum *frustum;
	float *data, prj[16], mv[16], clip[16], t;

	frustum = store[id];

	data = tlGroupArray(*frustum);

	glGetFloatv(GL_PROJECTION_MATRIX, prj);
	glGetFloatv(GL_MODELVIEW_MATRIX, mv);

	clip[0] = mv[0] * prj[0] + mv[1] * prj[4] + mv[2] * prj[8] + mv[3] * prj[12];
	clip[1] = mv[0] * prj[1] + mv[1] * prj[5] + mv[2] * prj[9] + mv[3] * prj[13];
	clip[2] = mv[0] * prj[2] + mv[1] * prj[6] + mv[2] * prj[10] + mv[3] * prj[14];
	clip[3] = mv[0] * prj[3] + mv[1] * prj[7] + mv[2] * prj[11] + mv[3] * prj[15];
	clip[4] = mv[4] * prj[0] + mv[5] * prj[4] + mv[6] * prj[8] + mv[7] * prj[12];
	clip[5] = mv[4] * prj[1] + mv[5] * prj[5] + mv[6] * prj[9] + mv[7] * prj[13];
	clip[6] = mv[4] * prj[2] + mv[5] * prj[6] + mv[6] * prj[10] + mv[7] * prj[14];
	clip[7] = mv[4] * prj[3] + mv[5] * prj[7] + mv[6] * prj[11] + mv[7] * prj[15];
	clip[8] = mv[8] * prj[0] + mv[9] * prj[4] + mv[10] * prj[8] + mv[11] * prj[12];
	clip[9] = mv[8] * prj[1] + mv[9] * prj[5] + mv[10] * prj[9] + mv[11] * prj[13];
	clip[10] = mv[8] * prj[2] + mv[9] * prj[6] + mv[10] * prj[10] + mv[11] * prj[14];
	clip[11] = mv[8] * prj[3] + mv[9] * prj[7] + mv[10] * prj[11] + mv[11] * prj[15];
	clip[12] = mv[12] * prj[0] + mv[13] * prj[4] + mv[14] * prj[8] + mv[15] * prj[12];
	clip[13] = mv[12] * prj[1] + mv[13] * prj[5] + mv[14] * prj[9] + mv[15] * prj[13];
	clip[14] = mv[12] * prj[2] + mv[13] * prj[6] + mv[14] * prj[10] + mv[15] * prj[14];
	clip[15] = mv[12] * prj[3] + mv[13] * prj[7] + mv[14] * prj[11] + mv[15] * prj[15];

	data[0] = clip[3] - clip[0];
	data[1] = clip[7] - clip[4];
	data[2] = clip[11] - clip[8];
	data[3] = clip[15] - clip[12];
	t = sqrtf(data[0] * data[0] + data[1] * data[1] + data[2] * data[2]);
	data[0] /= t;
	data[1] /= t;
	data[2] /= t;
	data[3] /= t;

	data[4] = clip[3] + clip[0];
	data[5] = clip[7] + clip[4];
	data[6] = clip[11] + clip[8];
	data[7] = clip[15] + clip[12];
	t = sqrtf(data[4] * data[4] + data[5] * data[5] + data[6] * data[6] );
	data[4] /= t;
	data[5] /= t;
	data[6] /= t;
	data[7] /= t;

	data[8] = clip[3] + clip[1];
	data[9] = clip[7] + clip[5];
	data[10] = clip[11] + clip[9];
	data[11] = clip[15] + clip[13];
	t = sqrtf(data[8] * data[8] + data[9] * data[9] + data[10] * data[10]);
	data[8] /= t;
	data[9] /= t;
	data[10] /= t;
	data[11] /= t;

	data[12] = clip[3] - clip[1];
	data[13] = clip[7] - clip[5];
	data[14] = clip[11] - clip[9];
	data[15] = clip[15] - clip[13];
	t = sqrtf(data[12] * data[12] + data[13] * data[13] + data[14] * data[14]);
	data[12] /= t;
	data[13] /= t;
	data[14] /= t;
	data[15] /= t;

	data[16] = clip[3] - clip[2];
	data[17] = clip[7] - clip[6];
	data[18] = clip[11] - clip[10];
	data[19] = clip[15] - clip[14];
	t = sqrtf(data[16] * data[16] + data[17] * data[17] + data[18] * data[18]);
	data[16] /= t;
	data[17] /= t;
	data[18] /= t;
	data[19] /= t;

	data[20] = clip[3] + clip[2];
	data[21] = clip[7] + clip[6];
	data[22] = clip[11] + clip[10];
	data[23] = clip[15] + clip[14];
	t = sqrtf(data[20] * data[20] + data[21] * data[21] + data[22] * data[22]);
	data[20] /= t;
	data[21] /= t;
	data[22] /= t;
	data[23] /= t;
}

int
mlFrustumPoint(unsigned int id, const float *point)
{
	MLFrustum *frustum;
	float *data;
	unsigned int f;

	frustum = store[id];

	data = tlGroupArray(*frustum);

	for(f = 0; f < 6; f++)
		if(data[f * 4 + 0] * point[0] + data[f *4 + 1] * point[1] + data[f * 4 + 2] * point[2] + data[f * 4 + 3] <= 0)
			return 0;
	return 1;
}

int
mlFrustumSphere(unsigned int id, const float *sphere)
{
	MLFrustum *frustum;
	float *data;
	unsigned int p;

	frustum = store[id];

	data = tlGroupArray(*frustum);

	for(p = 0; p < 6; p++)
		if(data[p * 4 + 0] * sphere[0] + data[p *4 + 1] * sphere[1] + data[p *4 + 2] * sphere[2] + data[p * 4 + 3] < -sphere[3])
			return 1;
	return 0;
}

int
mlFrustumTriangle(unsigned int id, const float *triangle)
{
	MLFrustum *frustum;
	float *data;
	unsigned int f, p;

	frustum = store[id];

	data = tlGroupArray(*frustum);

	for(f = 0; f < 6; f++)
	{
		for(p = 0; p < 3; p++)
			if(data[f * 4 + 0] * triangle[p * 3 + 0] + data[f * 4 + 1] * triangle[p * 3 + 1] + data[f * 4 + 2] * triangle[p * 3 + 2] + data[f * 4 + 3] > 0)
				break;
		if(p == 3) return 0;
	}
	return 1;
}

int
mlFrustumCube(unsigned int id, const float *cube)
{
	MLFrustum *frustum;
	float *data;
	unsigned int p;

	frustum = store[id];

	data = tlGroupArray(*frustum);

	for(p = 0; p < 6; p++)
	{
		if(data[p * 4 + 0] * (cube[0] - cube[3]) + data[p * 4 + 1] * (cube[1] - cube[3]) + data[p * 4 + 2] * (cube[2] - cube[3]) + data[p * 4 + 3] > 0)
			continue;
		if(data[p * 4 + 0] * (cube[0] + cube[3]) + data[p * 4 + 1] * (cube[1] - cube[3]) + data[p * 4 + 2] * (cube[2] - cube[3]) + data[p * 4 + 3] > 0)
			continue;
		if(data[p * 4 + 0] * (cube[0] - cube[3]) + data[p * 4 + 1] * (cube[1] + cube[3]) + data[p * 4 + 2] * (cube[2] - cube[3]) + data[p * 4 + 3] > 0)
			continue;
		if(data[p * 4 + 0] * (cube[0] + cube[3]) + data[p * 4 + 1] * (cube[1] + cube[3]) + data[p * 4 + 2] * (cube[2] - cube[3]) + data[p * 4 + 3] > 0)
			continue;
		if(data[p * 4 + 0] * (cube[0] - cube[3]) + data[p * 4 + 1] * (cube[1] - cube[3]) + data[p * 4 + 2] * (cube[2] + cube[3]) + data[p * 4 + 3] > 0)
			continue;
		if(data[p * 4 + 0] * (cube[0] + cube[3]) + data[p * 4 + 1] * (cube[1] - cube[3]) + data[p * 4 + 2] * (cube[2] + cube[3]) + data[p * 4 + 3] > 0)
			continue;
		if(data[p * 4 + 0] * (cube[0] - cube[3]) + data[p * 4 + 1] * (cube[1] + cube[3]) + data[p * 4 + 2] * (cube[2] + cube[3]) + data[p * 4 + 3] > 0)
			continue;
		if(data[p * 4 + 0] * (cube[0] + cube[3]) + data[p * 4 + 1] * (cube[1] + cube[3]) + data[p * 4 + 2] * (cube[2] + cube[3]) + data[p * 4 + 3] > 0)
			continue;
		return 0;
	}
	return 1;
}

int
mlFrustumPolygon(unsigned int id, const float *points, unsigned int npoints)
{
	MLFrustum *frustum;
	float *data;
	unsigned int f, p;

	frustum = store[id];

	data = tlGroupArray(*frustum);

	for(f = 0; f < 6; f++)
	{
		for(p = 0; p < npoints; p++)
			if(data[f * 4 + 0] * points[p * 3] + data[f * 4 + 1] * points[p * 3 + 1] + data[f * 4 + 2] * points[p * 3 + 2] + data[f * 4 + 3] > 0)
				break;
		if(p == npoints) return 0;
	}
	return 1;
}
